#include <stdlib.h>
#include <stdio.h>
#include <string.h>

#include "common.h"
#include "quadtree.h"

long num_nodes = 0;

int is_leaf( quadtree_node *node )
{
    return node->num_children == 0;
}

bounding_box* quadrant_bounding_box( quadrant q, quadtree_node *parent )
{
    bounding_box *b = ( bounding_box * ) malloc( sizeof(bounding_box) );

    double x_origin = parent->box->x;
    double y_origin = parent->box->y;
    double x_right = x_origin + parent->box->length;
    double y_top = y_origin + parent->box->length;
    double x_mid = x_origin + ((x_right - x_origin) / 2 );
    double y_mid = y_origin + ((y_top - y_origin) / 2 );

    double length = parent->box->length / 2;

    if( q == BOT_LEFT )
    {
        b->x = x_origin;
        b->y = y_origin;
    } 
    if( q == BOT_RIGHT )
    {
        b->x = x_mid;
        b->y = y_origin;
    }
    if( q == TOP_RIGHT )
    {
        b->x = x_mid;
        b->y = y_mid;
    }
    if( q == TOP_LEFT )
    {
        b->x = x_origin;
        b->y = y_mid;
    }
    
    b->length = length;
    return b;

}

quadrant particle_quadrant( particle_t *p, quadtree_node *node )
{
    
    double x_origin = node->box->x;
    double y_origin = node->box->y;
    double x_right = x_origin + node->box->length;
    double y_top = y_origin + node->box->length;
    double x_mid = x_origin + ((x_right - x_origin) / 2 );
    double y_mid = y_origin + ((y_top - y_origin) / 2 );

    if( p->x < x_mid && p->y < y_mid )
        return BOT_LEFT;
    if( p->x >= x_mid && p->y < y_mid )
        return BOT_RIGHT;
    if( p->x >= x_mid && p->y >= y_mid )
        return TOP_RIGHT;    
    if( p->x < x_mid && p->y >= y_mid )
        return TOP_LEFT;
 
    return BOT_LEFT; // FIXME: this is an error, shouldn't happen
}

void create_child( quadtree_node *node, quadrant q)
{
    quadtree_node *child = ( quadtree_node * ) malloc( sizeof(quadtree_node) );
    bounding_box *box = quadrant_bounding_box( q, node );
    child->id = num_nodes;
    num_nodes++;
    child->box = box;
    child->q = q;
    child->num_children = 0;
    child->num_particles = 0;
    child->com_x = 0.0;
    child->com_y = 0.0;
    child->total_mass = 0.0;
    child->level = node->level + 1;
    child->parent = node;
    
    node->children[q] = child;
    node->num_children++;
}

void create_children( quadtree_node *node )
{ 
    for( int i = 0; i < 4; i++ )
    {
       create_child( node, ( quadrant ) i ); 
    }  
}

void quad_insert( particle_t *particle, quadtree_node *node )
{

    quadrant new_pq = particle_quadrant( particle, node ); 
    if( node->num_particles > 1 )
    {
        quad_insert( particle, node->children[new_pq] );
        node->num_particles++;
    }
    else if( node->num_particles == 1 )
    {
        create_children( node );
        particle_t *old_particle = node->particle;
        quadrant old_pq = particle_quadrant( old_particle, node );
        node->children[old_pq]->particle = node->particle;
        node->children[old_pq]->num_particles++;
        node->particle = 0;
        quad_insert( particle, node->children[new_pq] );
        node->num_particles++;
    }
    else if( node->num_particles == 0 )
    {  
        node->particle = particle;
        node->num_particles++;
    }
}

void eliminate_empty( quadtree_node *root )
{
    quad_queue q; 
    q.push( root );
    while( !q.empty( ) )
    {
        quadtree_node *n = ( quadtree_node * ) q.front( );
        q.pop( );
        for( int i = 0; i < 4; i++ )
        {
            if( n->num_children > 0 && n->children[i] != 0 )
            {
                if( n->children[i]->num_particles == 0 )
                { 
                    free( n->children[i] );
                    n->children[i] = 0;
                    n->num_children--;
                }
                else
                {
                    q.push( n->children[i] );
                }
            }
        }
    }
}

void init_root( quadtree_node *root, double size )
{
    root->id = num_nodes;
    num_nodes++;
    root->box = ( bounding_box * ) malloc( sizeof(bounding_box) );
    root->box->x = 0.0;
    root->box->y = 0.0;
    root->box->length = size;
    root->num_particles = 0;
    root->level = 0;
    root->num_children = 0;
    root->com_x = 0.0;
    root->com_y = 0.0;
    root->total_mass = 0.0;
}

void build_quadtree( quadtree_node *root, particle_t *particles, int n, double size )
{ 
    init_root( root, size ); 
    for( int i = 0; i < n; i++ )
    {
        quad_insert( &particles[i], root );
    }
    eliminate_empty( root );
    //print_quadtree( root );
}

void print_quadtree( quadtree_node *root )
{
    fprintf( stderr, "Printing tree\n" );
    quad_queue q;
    q.push( root );
    int last_level = root->level; 
    fprintf( stderr, "\nlevel %d: \n", root->level );
    while( !q.empty( ) )
    {
        quadtree_node *n = ( quadtree_node * ) q.front( );
        q.pop( ); 
        if( n->level != last_level )
        {
            fprintf( stderr, "\nlevel %d: \n", n->level );
            last_level = n->level;
        }
        fprintf( stderr, "(n%ld, %ld)  ", n->id, n->num_particles );
        for( int i = 0; i < 4; i++ )
        {
            if( n->num_children > 0 && n->children[i] != 0 )
            {
                q.push( n->children[i] ); 
            }
        }

    }
    fprintf( stderr, "\n" );
}

void print_particles( particle_t *p, int n )
{
    fprintf( stderr, "Printing particles\n" );
    for( int i = 0; i < n; i++ )
    {
        fprintf( stderr, "\t(%f, %f)\n", p[i].x, p[i].y );
    }
}
